﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using TWiStEr.Math;

namespace Phys.Model.Affectors {
	public class CollisionAffector : IAffector {
		public bool IsFixed { get; set; }
		internal bool IsCollided;
		public void Affect(IObject obj1, double seconds) {
			Debug.Assert(obj1.Affectors.Contains(this), "Object does not contains this Affector.");
			CollisionAffector aff1 = this;
			foreach (IObject obj2 in obj1.World.AllPrimitives.Where(o => o.Affectors.OfType<CollisionAffector>().Any())) {
				if (obj1 != obj2 && (AreColliding(obj1, obj2, seconds) ?? false)) {
					CollisionAffector aff2 = obj2.Affectors.OfType<CollisionAffector>().Single();
					if (aff1.IsFixed && aff2.IsFixed) continue; // two fixed objects can't collide
					if (aff1.IsFixed) {
						; // CollideFixed(obj1, obj2);
					} else if (aff2.IsFixed) {
						if (!aff1.IsCollided) CollideFixed(obj2, obj1);
					} else {// Neither fixed 
						if(!aff1.IsCollided && !aff2.IsCollided) Collide(obj1, obj2);
					}
				}
			}
		}

		private void CollideFixed(IObject fix, IObject obj) {
			if (fix is SphereObject && obj is SphereObject)
				CollideFixed(fix, obj, obj.Position - fix.Position, obj.Velocity);
			if (fix is RectObject)
				CollideFixed(fix, obj, ((RectObject)fix).Normal, obj.Velocity);
		}
		private void CollideFixed(IObject fix, IObject obj, Vector normal, Vector vel) {
			normal.Normalize();
			Vector v1x = Vector.Normalize(vel).Project(normal);
			Vector v1y = Vector.Normalize(vel).Perpendicular(normal);
			Vector anti = -vel; // == new Vector(-v1x + -v1y, vel.Length);
			Vector mirrored = new Vector(-v1x + v1y, vel.Length);
			//if (fix is RectObject) mirrored = -vel;
			obj.Velocity = mirrored * MaterialAffector.GetElasticity(obj);
			obj.Affectors.OfType<CollisionAffector>().Single().IsCollided = true;
			// pattan
			// obj.Velocity = ((obj.Position - fix.Position).Normalize() + Vector.Normalize(obj.Velocity)).Normalize() / 100;
			// folyás
			// obj.Velocity = ((obj.Position - fix.Position).Normalize() + Vector.Normalize(obj.Velocity)).Normalize() / 1000;
		}

		// lim_{obj2.Mass -> ∞) (obj1.Mass - obj2.Mass) / (obj1.Mass + obj2.Mass) =(L'Hopital) (0-1) / (0+1) = -1
		// lim_{obj2.Mass -> ∞) (2 * obj2.Mass) / (obj1.Mass + obj2.Mass) =(L'Hopital) 2 / (0+1) = 2
		// lim_{obj2.Velocity -> 0) x.Project(obj2.Velocity) = obj2.Velocity * DotProduct(x, obj2.Velocity) / obj2.Velocity.Length
		//	let v = obj2.Velocity;	= v * (x.v) / |v| = v * (x.X * v.X + x.Y * v.Y + x.Z * v.Z) / √(v.X^2 + v.Y^2 + v.Z^2)
		//							= ...?
		private static void Collide(IObject obj1, IObject obj2) {
			/// http://wp.freya.no/simple-sphere-sphere-collision-detection-and-collision-response
			Vector normal = (obj1.Position - obj2.Position).Normalize();
			Vector v1x = Vector.Normalize(obj1.Velocity).Project(normal);
			Vector v1y = Vector.Normalize(obj1.Velocity).Perpendicular(normal);
			normal = -normal;
			Vector v2x = Vector.Normalize(obj2.Velocity).Project(normal);
			Vector v2y = Vector.Normalize(obj2.Velocity).Perpendicular(normal);
			if (obj1.Velocity.Length < double.Epsilon) { v1x = Vector.Zero; v1y = Vector.Zero; }
			if (obj2.Velocity.Length < double.Epsilon) { v2x = Vector.Zero; v2y = Vector.Zero; }
			Vector newVel1 = v1x * (obj1.Mass - obj2.Mass) / (obj1.Mass + obj2.Mass) + v2x * (2 * obj2.Mass) / (obj1.Mass + obj2.Mass) + v1y;
			Vector newVel2 = v1x * (2 * obj1.Mass) / (obj1.Mass + obj2.Mass) + v2x * (obj2.Mass - obj1.Mass) / (obj1.Mass + obj2.Mass) + v2y;
			newVel1.Length = (obj2.Velocity / 1 * obj2.Mass).Length;
			newVel2.Length = (obj1.Velocity / 1 * obj1.Mass).Length;
			//if (Vector.DotProduct(normal, newVel1) < 0)
			
			obj1.Velocity = newVel1;
			obj2.Velocity = newVel2;

			obj1.Affectors.OfType<CollisionAffector>().Single().IsCollided = true;
			obj2.Affectors.OfType<CollisionAffector>().Single().IsCollided = true;
		}
		public bool? AreColliding(IObject obj1, IObject obj2, double seconds) {
			if (obj1 is SphereObject && obj2 is SphereObject) {
				SphereObject s1 = (SphereObject)obj1, s2 = (SphereObject)obj2;
				Vector relPos = s1.Position - s2.Position;
				double dist = relPos.Length;
				double minDist = s1.Radius + s2.Radius;
				return dist <= minDist;
			}
			if (obj1 is SphereObject && obj2 is RectObject) { var temp = obj1; obj1 = obj2; obj2 = temp; }
			if (obj1 is RectObject && obj2 is SphereObject) {
				RectObject rect = (RectObject)obj1;
				SphereObject sph = (SphereObject)obj2;
				double dist = (sph.Position - rect.Position) * rect.Normal;
				if (dist < sph.Radius) {
					return true; // PointOnRect(rect, sph.Position + -rect.Normal * dist);
				}
				//double D = Vector.DotProduct(-rect.Normal, rect.Position);
				//Where before = WhereIsThePoint(rect.Normal, D, sph.Position);
				//Where after = WhereIsThePoint(rect.Normal, D, sph.Position + sph.Velocity);
				//if (before == after) return false;
				//// not implemented
				//Vector intersectionPoint = GetIntersection(rect.Normal, D, sph.Position, sph.Position + sph.Velocity * seconds);
				//return PointOnRect(rect, intersectionPoint);
				/// http://nehe.gamedev.net/data/lessons/lesson.asp?lesson=30
				//double DotProduct = Vector.DotProduct(sph.Velocity, rect.Normal);			// Dot Product Between Plane Normal And Ray Direction
				//// Determine If Ray Parallel To Plane
				//if (Math.Abs(DotProduct) < double.Epsilon * 10)
				//    return false;
				//double l2 = Vector.DotProduct(rect.Normal, rect.Position - sph.Position) / DotProduct;
				//if (l2 < -double.Epsilon * 10) // Test If Collision Behind Start
				//    return false;
				//return true;

				/// http://www.edenwaith.com/products/pige/tutorials/collision.php
				//double distance = Vector.DotProduct(rect.Normal, sph.Position);
				//return Math.Abs(distance) < sph.Radius;
			}
			return null;
		}

		private bool? PointOnRect(RectObject rect, Vector intersectionPoint) {
			return PointOnPolygon(intersectionPoint, new Vector(-rect.Width / 2, 0, -rect.Width / 2), new Vector(rect.Width / 2, 0, -rect.Height / 2),
				new Vector(rect.Width / 2, 0, rect.Height / 2), new Vector(-rect.Width / 2, 0, rect.Height / 2));
		}
		private bool? PointOnPolygon(Vector point, params Vector[] vertices) {
			double sum = 0;
			for (int i = 0; i < vertices.Length; i++) {
				sum += Vector.AngleBetween(point, vertices[i], vertices[(i + 1) % vertices.Length]);
			}
			Console.WriteLine(sum);
			return Math.Abs(sum - 2 * Math.PI) < Math.PI / 1000;
		}

		private Vector GetIntersection(Vector normal, double distance, Vector position, Vector destination) {
			var ray = destination - position;
			var t = -(Vector.DotProduct(normal, position) + distance) / Vector.DotProduct(normal, ray);
			return position + (ray * t);
		}

		enum Where { Front, Back, On };
		private static Where WhereIsThePoint(Vector normal, double distance, Vector destination) {
			double p = Vector.DotProduct(normal, destination) + distance;
			if (p > 0.0) return Where.Front;
			else if (p < 0.0) return Where.Back;
			return Where.On;
		}
	}
}
